package eu.robico.simulator.simulatorinterface.actionpanel.objects;

import java.awt.Color;
import java.awt.Graphics2D;
import java.util.ArrayList;
import java.util.List;

import eu.robico.simulator.simulatorinterface.MainController;
import eu.robico.simulator.simulatorinterface.RobotStatus;
import eu.robico.simulator.simulatorinterface.actionpanel.ActionPanel;
import eu.robico.simulator.simulatorinterface.actionpanel.Position;
import eu.robico.simulator.simulatorinterface.actionpanel.objects.CollisionSensor.CollisionSensorPosition;
import eu.robico.simulator.simulatorinterface.actionpanel.objects.ColorLaneSensor.ColorLaneSensorPosition;
import eu.robico.simulator.simulatorinterface.communication.Protocol.WheelStatus;
import eu.robico.simulator.simulatorinterface.utils.Toolkit;

public class Robot implements ActionPanelObject, UpdateableActionPanelObject
{
	private ActionPanel actionPanel;
	private double x, y, rotation = 0; 
	private int length = 80, width = 50;
	
	private List<RobotComponent> robotComponents = new ArrayList<>();
	
	/**
	 * Represents a on-screen robot object
	 * @author A.J. Janssen
	 * @version 0.1
	 *
	 */
	private class RobotObject
	{
		public void paint(Graphics2D graphics) 
		{
			graphics.setColor(Color.YELLOW);
			graphics.rotate(Math.toRadians(rotation), x, y);
			graphics.fillRect((int) x - (length / 2), (int) y - (width / 2), length, width);
			paintComponents(graphics);
		}
		
		private void paintComponents(Graphics2D graphics) 
		{
			//Paint components
			for (RobotComponent robotComponent : robotComponents)
				robotComponent.paint(graphics);
		}
	}
	
	private Robot.RobotObject robotObject;
	
	public Robot(ActionPanel actionPanel, Position position, double rotation)
	{
		this.actionPanel = actionPanel;
		this.x = position.x;
		this.y = position.y;

		//Add sensors
		addCollisionSensor();
		addColorLaneSensors();
//		addSideExitSensors();
		addDistanceSensor();
		addCrossRoadsSensor();
		//Set component distance
		setComponentsDistance();
		
		//Alter rotation
		alterRotation(rotation);
		
		//Instantiate RobotObject
		robotObject = this.new RobotObject();
	}
	
	private void addCollisionSensor()
	{
		Position position = new Position(x + (length / 2) - 2, y);
		robotComponents.add(new CollisionSensor(this, position, 0, CollisionSensorPosition.FRONT));
	}
	
	private void addColorLaneSensors()
	{
		Position 	leftSensor = new Position(x + (length / 3), y - (width / 3)),
					rightSensor = new Position(x + (length / 3), y + (width / 3));
		robotComponents.add(new ColorLaneSensor(this, leftSensor, 330, ColorLaneSensorPosition.CENTRALLEFT));
		robotComponents.add(new ColorLaneSensor(this, rightSensor, 30, ColorLaneSensorPosition.CENTRALRIGHT));
	}
	
	private void addSideExitSensors()
	{
		Position	leftSensor = new Position(x + (length / 2), y - (width / 2)),
					rightSensor = new Position(x + (length / 2), y + (width / 2));
		robotComponents.add(new ColorLaneSensor(this, leftSensor, 330, ColorLaneSensorPosition.OUTERLEFT));
		robotComponents.add(new ColorLaneSensor(this, rightSensor, 30, ColorLaneSensorPosition.OUTERRIGHT));
	}
	
	private void addDistanceSensor()
	{
		robotComponents.add(new DistanceSensor(this, new Position(x + (length / 2) - 8, y), 0));
	}
	
	private void addCrossRoadsSensor()
	{
		robotComponents.add(new CrossRoadsSensor(this, new Position(x + (length / 2), y), 0));
	}
	
	private void setComponentsDistance()
	{
		for (RobotComponent component : robotComponents)
			component.setDistanceToRobotCenter(Toolkit.getDistance(getPosition(), component.getPosition()));
	}
	
	public void update() 
	{
		//Get next action
		RobotStatus status = MainController.getInstance().getRobotStatusCopy();
		WheelStatus leftWheelStatus = status.getLeftWheelStatus(),
					rightWheelStatus = status.getRightWheelStatus();
		double speed = 0, rotation = 0;
		if (leftWheelStatus == WheelStatus.FORWARD && rightWheelStatus == WheelStatus.FORWARD)
			speed = 1;
		else if (leftWheelStatus == WheelStatus.BACKWARD && rightWheelStatus == WheelStatus.BACKWARD)
			speed = -0.5;
		else if (leftWheelStatus == WheelStatus.FORWARD && rightWheelStatus == WheelStatus.BACKWARD)
			rotation = 0.25;
		else if (leftWheelStatus == WheelStatus.BACKWARD && rightWheelStatus == WheelStatus.FORWARD)
			rotation = -0.25;
		//Execute action
		move(speed);
		alterRotation(rotation);
		
		//Update components
		updateComponents();
	}
	
	private void updateComponents()
	{
		updateComponentsCenterPoints();

		for (RobotComponent robotComponent : robotComponents)
			robotComponent.update();
	}
	
	private void updateComponentsCenterPoints()
	{
		for (RobotComponent robotComponent : robotComponents)
		{
			//Calculate component coordinates
			double x = this.x + (Math.cos(Math.toRadians(robotComponent.getRotation())) * robotComponent.getDistanceToRobotCenter());
			double y = this.y + (Math.sin(Math.toRadians(robotComponent.getRotation())) * robotComponent.getDistanceToRobotCenter());
			//Set component center point
			robotComponent.setTrueCenter(new Position(x, y));
		}
	}
	
	/**
	 * Alters the direction of the robot with the specified amount of degrees. 
	 * @param alteration Alteration in degrees.
	 */
	public void alterRotation(double alteration)
	{
		rotation = alterRotationBy(rotation, alteration);
		for (RobotComponent robotComponent : robotComponents)
			robotComponent.setRotation(
				alterRotationBy(
					robotComponent.getRotation(), alteration
			));
	}
	
	private double alterRotationBy(double rotation, double alteration)
	{
		double result = rotation + alteration;
		while (result < 0 || result > 360)
			result = (result < 0) ? result + 360 : result - 360;
		return result;
	}
	
	/**
	 * Moves the robot in its current direction with the specified speed.
	 * @param speed The speed the robot should move with, can be negative for a backward movement.
	 */
	private void move(double speed)
	{
		//Calculate speed
		double xSpeed = (Math.cos(Math.toRadians(rotation)) * speed);
		double ySpeed = (Math.sin(Math.toRadians(rotation)) * speed);
		//Alter position
		alterPosition(xSpeed, ySpeed);
	}
	
	private void alterPosition(double xDiff, double yDiff)
	{
		x = x + xDiff;
		y = y + yDiff;
		for (RobotComponent robotComponent : robotComponents)
		{
			robotComponent.setX(robotComponent.getX() + xDiff);
			robotComponent.setY(robotComponent.getY() + yDiff);
		}
	}
	
	public ActionPanel getActionPanel()
	{
		return actionPanel;
	}
	
	public Position getPosition()
	{
		return new Position(x, y);
	}
	
	@Override
	public void paint(Graphics2D graphics) 
	{
		robotObject.paint(graphics);
	}

}
